/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : imu_preintegration.h
 *@brief   : imu preintegration
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/8/24
 *
 */

#ifndef IMU_PREINTEGRATION_H_
#define IMU_PREINTEGRATION_H_
#include <iostream>
#include <vector>

#include "common/state.h"
#include "common/se2.h"

namespace preintegration {

struct ImuOptions {
   ImuOptions() {}
   double init_bg_ = 0.0;  // 初始零偏
   Eigen::Vector2d init_ba_ = Eigen::Vector2d::Zero();  // 初始零偏
   double noise_gyro_ = 1e-2;       // 陀螺噪声，标准差
   double noise_acce_ = 0.1;       // 加计噪声，标准差
};

class ImuPreIntegration {
public:
   ImuPreIntegration(ImuOptions options = ImuOptions());

   void Integrate(const Eigen::Vector3d& last_imu, const Eigen::Vector3d& curr_imu, const double& dt);

   State Predict(const State& last_state);

   SO2 GetDeltaRotation(const double &bg);
   Eigen::Vector2d GetDeltaVelocity(const double &bg, const Eigen::Vector2d &ba);
   Eigen::Vector2d GetDeltaPosition(const double &bg, const Eigen::Vector2d &ba);
   void Clear(const ImuOptions& options);

public:
    double dt_ = 0; 
    Eigen::Matrix<double, 5, 5> cov_ = Eigen::Matrix<double, 5, 5>::Zero();
    Eigen::Matrix3d noise_gyro_acce_ = Eigen::Matrix3d::Zero();

    Eigen::Vector2d ba_ = Eigen::Vector2d::Zero();
    double bg_;

    SO2 dR_;
    Eigen::Vector2d dv_ = Eigen::Vector2d::Zero();
    Eigen::Vector2d dp_ = Eigen::Vector2d::Zero();  

    double dR_dbg_ = 0;
    Eigen::Vector2d dV_dbg_ = Eigen::Vector2d::Zero();
    Eigen::Matrix2d dV_dba_ = Eigen::Matrix2d::Zero();
    Eigen::Vector2d dP_dbg_ = Eigen::Vector2d::Zero();
    Eigen::Matrix2d dP_dba_ = Eigen::Matrix2d::Zero();               
};

}

#endif 
